#ifndef MZ_FLITER
#define MZ_FLITER

class KalmanFilter
{
public:
    KalmanFilter() 
    {
        // 初始化卡尔曼滤波器参数
        processNoise_ = 0.001;
        measurementNoise_ = 0.01;

        // 初始化状态变量和协方差矩阵
        state_ = 0;
        covariance_ = 0;
    }
    KalmanFilter(float processNoise, float measurementNoise) 
    {
        // 初始化卡尔曼滤波器参数
        processNoise_ = processNoise;
        measurementNoise_ = measurementNoise;

        // 初始化状态变量和协方差矩阵
        state_ = 0;
        covariance_ = 0;
    }

    float filter(float measurement) 
    {
        // 预测步骤
        float predictedState = state_;
        float predictedCovariance = covariance_ + processNoise_;

        // 更新步骤
        float kalmanGain = predictedCovariance / (predictedCovariance + measurementNoise_);
        float updatedState = predictedState + kalmanGain * (measurement - predictedState);
        float updatedCovariance = (1 - kalmanGain) * predictedCovariance;

        // 更新滤波后的状态和协方差
        state_ = updatedState;
        covariance_ = updatedCovariance;

        return updatedState;
    }

private:
    float state_;               // 状态变量
    float covariance_;          // 协方差矩阵
    float processNoise_;        // 系统噪音方差
    float measurementNoise_;    // 测量噪音方差
};

#endif